/* 电机协议---第一版 */
#ifndef DECODEMOTOR_H
#define DECODEMOTOR_H

#include <iostream>
#include <vector>
#include <string>
#include "src/base/errors.h"

typedef  unsigned char uchar;
typedef  unsigned short u16;
typedef  unsigned long ulong;

enum LimitOperate{TRIGGER,RELEASE};
enum MotorType{MOTORX,MOTORZ};      // X方向   Z方向
enum LedType{LED1,LED2,LED3};
enum Control{OPEN,CLOSE};

enum ControlCMD{
    T_MOTOR_WISE = 0xC1,    // 顺时针
    T_MOTOR_ANTI = 0xC2,    // 逆时针
    T_MOTOR_BACK = 0xC4,    // 返回参数
    T_MOTOR_SLEEP = 0xC3    // 休眠阶段
};

/**
 * @brief The LinkPara struct
 * @details 当前电机的链接参数
 */
struct LinkPara{
        int bau = 9600;
        int stopBit = 8;
        std::string port;
};

/**
 * @brief The TransPara struct
 * @details 当前电机的转换参数
 */
struct TransPara{
        int  pulseToCircle = 1000;  // 一圈对应的脉冲
        int  circleToMM     = 0;    // 一圈对应多少mm
};


class MotorFcs{

public:
    void init(){
        stopCMD.push_back(0xa5);
        stopCMD.push_back(0xff);
        stopCMD.push_back(0x00);
        stopCMD.push_back(0xff);

        openLight.push_back(0xa5);
        openLight.push_back(0xc1);

        openLight.push_back(0x00);
        openLight.push_back(0x00);
        openLight.push_back(0x00);
        openLight.push_back(0xff);
    }

    /**
     * @brief packageMove
     * @param isWise 起头是否顺时针
     * @param length    start
     * @param sleepTime 休眠时间second
     * @return
     */
    static std::vector<uchar> packageMove( bool isWise,int length,int sleepTime ,double freq,bool isms=true,int nextTime = 1000){
        std::vector<uchar> __cmd;

        __cmd.push_back(0xA5);
        __cmd.push_back(0xA5);
        uchar step;     //定义平均的休眠时间
        int gains = 0;  // 定义总补偿
        int has_gains = 0;  // 定义已经产生的补偿
        int allSleep = abs(1000.00 / freq);
        if(length*4 >= allSleep){
            step = 0x01;
        }else{
            step = allSleep / (length*4);
            gains = allSleep - length*4*step;
        }

        // std::cout <<"补偿时间:"<<gains <<"\t"<<allSleep<<std::endl;

        // 到达阶段
        for(int i = 0;i<length;i++){
            __cmd.push_back(isWise?0xA1:0xB1);
            // 最后一位做延时
            if(length-1 == i){
                (*__cmd.end()) = isms?0xA1:0xA6;
                __cmd.push_back(sleepTime %  0x100);
            }else{
                if(has_gains < gains / 4 ){
                    __cmd.push_back(step+1);
                    has_gains++;
                }else{
                    __cmd.push_back(step);
                }
            }
        }
        gains -= has_gains;
        has_gains = 0;

        // 返回阶段
        for(int i = 0;i<length*2;i++){
            __cmd.push_back(isWise?0xB1:0xA1);
            // 最后一位做延时
            if((length*2-1) == i){
                (*__cmd.end()) = isms?0xB1:0xB6;
                __cmd.push_back(sleepTime %  0x100);
            }else{
                if(has_gains <= gains ){
                    __cmd.push_back(step+1);
                    has_gains++;
                }else{
                    __cmd.push_back(step);
                }

            }
        }

        gains -= has_gains;
        has_gains = 0;
        // 到达阶段
        for(int i = 0;i<length;i++){
            __cmd.push_back(isWise?0xA1:0xB1);
            if(has_gains < gains  ){
                __cmd.push_back(step+1);
                has_gains++;
            }else{
                __cmd.push_back(step);
            }
        }
        gains -= has_gains;
        has_gains = 0;
        if(10 < gains){
            std::cout <<"增益补偿误差时间:"<<gains <<"ms"<<std::endl;
        }

//        __cmd.push_back(0xD1);
//        __cmd.push_back(0xFF);

        __cmd.push_back(0xFF);
        __cmd.push_back(0xFF);


        return __cmd;
    }

    /**
     * @brief mmToHex
     * @return
     * @details 将mm单位转换为HEX字段
     * @part [0] 高8位
     * @part [1] 低8位
     */
    static uchar* mmToHex(int length){
        uchar ret[2];
        ret[0] = length  /  0x100;
        ret[1] = length  %  0x100;
        return  ret;
    }

    /**
     * @brief packageLED
     * @param lt
     * @param c
     * @return
     * @details 获取LED对应的操作
     */
    static std::vector<uchar> packageLED(LedType lt,Control c){
        std::vector<unsigned char> __cmd;
        __cmd.push_back(0xA5);

        if(lt == LED1)
            __cmd.push_back(0xC1);
        else if (lt == LED2) {
            __cmd.push_back(0xC2);
        }else
            __cmd.push_back(0xC3);
        __cmd.push_back( c == OPEN ? 0xF1 : 0xF0);

        __cmd.push_back(0x00);
        __cmd.push_back(0x00);
        __cmd.push_back(0x00);
        __cmd.push_back(0xff);
        return __cmd;
    }

    /**
     * @brief packageEEP
     * @param control
     * @param cmd1
     * @param cmd2
     * @param cmd3
     * @param cmd4
     * @param cmd5
     * @param sleep
     * @param tick
     * @return
     * @details 获取写EEP的命令
     */
    static std::vector<uchar> packageEEP(ControlCMD control,uint cmd1,uint cmd2,uint cmd3,
                                         uint cmd4,uint cmd5,uint sleep,uint tick = 0){
        std::vector<unsigned char> __cmd;
        __cmd.push_back(0xA5);
        __cmd.push_back(control);
        __cmd.push_back(tick);
        __cmd.push_back(sleep);
        __cmd.push_back(cmd1);
        __cmd.push_back(cmd2);
        __cmd.push_back(cmd3);
        __cmd.push_back(cmd4);
        __cmd.push_back(cmd5);
        return __cmd;
    }

    static void printVect(std::vector<uchar> &m){
        for(auto f:m){
            printf("%2X",f);
        }
        std::cout  << std::endl;
    }

    static void printVect(std::vector<u16> &m){
        for(auto f:m){
            printf("%4X",f);
        }
        std::cout  << std::endl;
    }


    /**
     * @brief getSwing
     * @param isStart 是否要开始启摆
     * @return
     */
    static std::vector<uchar> getSwing(bool isStart){
        std::vector<unsigned char> __cmd;
        __cmd.push_back(0xA5);
        __cmd.push_back(isStart ? 0x00 : 0x00);
        __cmd.push_back(0x00);
        __cmd.push_back(0x00);
        __cmd.push_back(0x00);
        __cmd.push_back(0x00);
        __cmd.push_back(0x00);
        __cmd.push_back(0x00);
        __cmd.push_back(0xFF);
        return __cmd;
    }

    std::vector<uchar> startCMD;        // 存放起摆指令
    std::vector<uchar> stopCMD;         // 存放停摆指令
    std::vector<uchar> openLight;

    LinkPara link;
    TransPara para;
};






#endif // DECODEMOTOR_H
